#pragma once

#define MAX_SIZE (10)
#define QUEUE_DATA_UNAVALIABLE (999999)

#include <mutex>
#include <numeric>

class CycleQueue
{
public:
    CycleQueue()
    {
        front = 0;
        size = 0;

        for (int i = 0; i < MAX_SIZE; ++i)
        {
            data[i] = 0;
        }
    }

    CycleQueue(const CycleQueue&cq)
    {
    }

    ~CycleQueue()
    {

    }

    void Update(double val)
    {
        data[front] = val;

        front = (++front) % MAX_SIZE;

        if (size < MAX_SIZE)
        {
            ++size;
        }
    }

    double GetRateOfChange()
    {
        if (size < MAX_SIZE / 2)
        {
            return QUEUE_DATA_UNAVALIABLE;
        }

        double data_rate[MAX_SIZE];
        for (int i = 0; i < size - 1; ++i)
        {
            data_rate[i] = data[i + 1] - data[i];

            if (data_rate[i] > 180)
            {
                data_rate[i] -= 360;
            }

            if (data_rate[i] < -180)
            {
                data_rate[i] += 360;
            }

            data_rate[i] = fabs(data_rate[i]);
        }

        double variance = GetVariance(data_rate, size - 1);

        return variance;
    }

    double GetDataVariance()
    {
        return GetVariance(data, size);
    }

private:
    double GetVariance(double val[], int size)
    {
        if (size < (MAX_SIZE / 2 - 1))
        {
            return QUEUE_DATA_UNAVALIABLE;
        }

        double mean = GetMean(val, size);

        double variance = 0.0;
        std::for_each(val, val + size, [&](const double d) {
            variance += pow(d - mean, 2);
        });
        variance /= size;

        return variance;
    }

    double GetMean(double val[], int size)
    {
        double sum = std::accumulate(val, val + size, 0.0);
        double mean = sum / size;

        return mean;
    }

private:
    double data[MAX_SIZE];

    int front;
    int size;
};

